#include "canbus/message.h"

/**
 * @namespace canbus
 * @brief canbus
 */
namespace canbus {

/**
 * @class CanMsg20
 * @brief Concrete can message that stores general vehicle command information
 */
class CanMsg20: public CanMsg {
public:
  CanMsg20():
  CanMsg()
  {
    id_ = 20;
    for(int i=0;i<8;i++) {
      *(data_+i) = 0;
    }
  }

  CanMsg20(uint8_t* value): 
  CanMsg(value)
  {
    id_ = 20;
    for(int i=0;i<8;i++) {
      *(data_+i) = 0;
    }
  }

  virtual ~CanMsg20() = default;

  void UpUpdateData() {}

  template <typename T>
  void DownUpdate(const T& input_data) {
    // 1st byte
    Byte control_word_1;
    control_word_1.set_bit(input_data.auto_mode, 0);
    control_word_1.set_bit(input_data.control_on ,1);
    control_word_1.set_bit(input_data.plc_reset ,2);
    control_word_1.set_bit(input_data.override_bypass ,3);
    control_word_1.set_bit(input_data.estop ,4);
    control_word_1.set_bit(input_data.parking ,5);
    control_word_1.set_bit(input_data.turn_en ,6);
    control_word_1.set_bit(0, 7);

    *data_ = control_word_1.get_byte();

    // 2nd byte
    Byte control_word_2;
    control_word_2.set_bit(input_data.gantry_en ,0);
    control_word_2.set_bit(input_data.gantry_brake ,1);
    control_word_2.set_bit(0, 2);
    control_word_2.set_bit(input_data.gantry_quickstop ,3);
    control_word_2.set_bit(input_data.hydraulic_front_en ,4);
    control_word_2.set_bit(input_data.hydraulic_back_en ,5);
    control_word_2.set_bit(input_data.hoist_en ,6);
    control_word_2.set_bit(input_data.spreader_en ,7);
    *(data_ + 1) = control_word_2.get_byte();

    // 8th byte
    Byte byte;
    heart_beat_ = heart_beat_>255?0:heart_beat_;
    byte.set_value(heart_beat_++);
    *(data_ + 7) = byte.get_byte();
  }

  static const int32_t ID;
  static const std::string NAME;

  friend class CanbusHandler;
private:
  uint16_t heart_beat_;
};

}